using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;

namespace Diversity.Robotics.Navigation
{
    /// <summary>
    /// Models the uncertainty in a beam distance sensor.
    /// </summary>
    public class BeamModel
    {
        private double _beamHitVariance = 80;

        public double BeamHitVariance
        {
            get { return _beamHitVariance; }
            set { _beamHitVariance = value; }
        }
        private double _beamShortDecay = 0.01;

        public double BeamShortDecay
        {
            get { return _beamShortDecay; }
            set { _beamShortDecay = value; }
        }
        private double _maxRange = 200; // cm
        private double _mixHit = 0.33;
        private double _mixShort = 0.35;
        private double _mixRandom = 0.3;
        private double _mixFailure = 0.02;
        private double[,] _probabilities;

        private double[,] _hitProbabilities;
        private double[,] _shortProbabilities;
        private double _randomProbability;

        public double MaxRange
        {
            get { return _maxRange; }
            set { _maxRange = value; }
        }

        public BeamModel() { }
        public BeamModel(double hitVariance, double shortDecay, double maxRange, double mixHit, double mixShort, double mixRand, double mixFailure)
        {
           // ProbabilityUtils = new ProbabilityUtils();
            _beamHitVariance = hitVariance;
            _beamShortDecay = shortDecay;
            _maxRange = maxRange;
            _mixHit = mixHit;
            _mixShort = mixShort;
            _mixRandom = mixRand;
            _mixFailure = mixFailure;
            Prepare();
        }

        public void Prepare() {

            // prepare lookup tables of probability given a test distance and hit distance
            int intMaxRange = (int)Math.Round(_maxRange);

            // do the calculations in real coordinates
            _hitProbabilities = new double[intMaxRange+1, intMaxRange+1];
            _shortProbabilities = new double[intMaxRange+1, intMaxRange+1];
            _randomProbability = 1.0;// / _maxRange;
            _probabilities = new double[intMaxRange+1, intMaxRange+1];

            Console.Out.WriteLine("Preparing Beam Probability lookup tables");
            for (int h = 0; h <= intMaxRange; h++)
            {
                double maxHitProbability = 0;
                double maxShort = 0;
               // double totalShortProbability = 0;
                for (int d = 0; d <= intMaxRange; d++)
                {
                    // calculate the hit prob
                    _hitProbabilities[h, d] = ProbabilityOfHit(d, h);
                    if (_hitProbabilities[h, d] > maxHitProbability)
                        maxHitProbability = _hitProbabilities[h, d];

                    // calculate the short prob
                    _shortProbabilities[h, d] = ProbabilityOfShort(d, h);
                    if (_shortProbabilities[h, d] > maxShort)
                        maxShort = _shortProbabilities[h, d];

                        //Console.Out.WriteLine("Raw short greater than 1 " +h + "," + d + "," + _shortProbabilities[h, d]);

               //     if(h==50)
               // {
                //    Console.Out.WriteLine("results: " + h + "," + d + "," + _hitProbabilities[h,d] + "," + _shortProbabilities[h,d]);
               // }
                }

                // noramlize to max of 1;
                for (int d = 0; d <= intMaxRange; d++)
                {
                    _hitProbabilities[h, d] = _hitProbabilities[h, d] / maxHitProbability;
                    _shortProbabilities[h, d] = _shortProbabilities[h, d]/maxShort;
                }
            }
            

            for (int h = 0; h <= intMaxRange; h++)
            {
                double maxProbability = 0;
                for (int d = 0; d <= intMaxRange; d++)
                {
                    double failureProbability = (d == intMaxRange ) ? 1 : 0;

                    // add the probabilities together according to the mixing factors
                    _probabilities[h,d] = (_mixHit * _hitProbabilities[h,d] +
                        _mixFailure * failureProbability +
                        _mixRandom * _randomProbability +
                        _mixShort * _shortProbabilities[h, d]);

                    if (_probabilities[h, d] > maxProbability) {
                        maxProbability = _probabilities[h, d];
                    }
                }
               
                if (maxProbability > 1)
                {
                    Console.Out.Write("Exceeded max prob - SHOULD NEVER HAPPEN " + maxProbability);
                }

                // re-normalize to max of one;
                for (int d = 0; d <= intMaxRange; d++)
                {
                    _probabilities[h, d] = _probabilities[h, d] / maxProbability;
                   // if (d == MaxRange)
                   // {
                    //    Console.Out.WriteLine("Probability of " + d + " in " + h + " is " + _probabilities[h, d]);
                   // }
                }
            }
        }

        #region Probability Decomposition
        /// <summary>
        /// Return a non-normalized probability of a hit
        /// </summary>
        /// <param name="distance">The test distance</param>
        /// <param name="hitDistance">The true distance</param>
        /// <returns></returns>
        private double ProbabilityOfHit(double distance, double hitDistance)
        {
            if (distance >= 0 && distance <= _maxRange)
            {
                return (1 / Math.Sqrt(2 * Math.PI * _beamHitVariance) * Math.Pow(Math.E, - Math.Pow(hitDistance - distance, 2) / (2 * _beamHitVariance)));
            }
            else
            {
                return 0;
            }
        }

        /// <summary>
        /// Returns the probability of a short
        /// </summary>
        /// <param name="distance">The test distance</param>
        /// <param name="hitDistance">The hit distance</param>
        /// <returns></returns>
        private double ProbabilityOfShort(double distance, double hitDistance)
        {
            if (hitDistance > 0 && distance >= 0 && distance <= hitDistance)
            {
                double normalizer = 1.0/ (1.0 - Math.Pow(Math.E, -_beamShortDecay * hitDistance));
                double val = _beamShortDecay * Math.Pow(Math.E, -_beamShortDecay * distance);
                return Math.Min(normalizer*val,1);
            }
            else
            {
                return 0;
            }
        }
        #endregion


        public double GetProbability(double distance, double hitDistance)
        {
            return _probabilities[(int)Math.Round(Math.Min(_maxRange,hitDistance)), (int)Math.Round(Math.Min(_maxRange,distance))];
        }

        /// <summary>
        /// Returns the array of probabilities at the given hit range.
        /// This just extracts an array from lookup table.
        /// </summary>
        /// <param name="range">The hit distance</param>
        /// <returns>An array of probabilities</returns>
        public double[] GetDistribution(double range)
        {
            int intRange = (int)Math.Round(range);
            if (intRange > _maxRange)
            {
                intRange = (int) Math.Round(MaxRange);
            }
            int numValues = _probabilities.GetLength(0);

            double[] distribution = new double[numValues];
            for (int i = 0; i < numValues; i++)
            {
                distribution[i] = _probabilities[intRange, i];
               //Console.Out.WriteLine("Range: " + intRange + " test: " + i + " prob: " + distribution[i]);
            }
            return distribution;
        }

        public Image GetDistributionImage(Image img, double range)
        {
            int intRange = (int)Math.Round(range);
            int numValues = _probabilities.GetLength(0);

            Graphics g = Graphics.FromImage(img);
            PointF[] points = new PointF[numValues];

            double maxProb = 0;
            double totalProb = 0;
            for (int i = 0; i < numValues; i++)
            {
                totalProb += _probabilities[intRange, i];
                if (_probabilities[intRange, i] > maxProb)
                {
                    maxProb = _probabilities[intRange, i];
                }
            }

            // do the full scale graph
            for (int i = 0; i < numValues; i++)
            {
                points[i] = new PointF((float)((img.Width * i * 1.0) / (numValues)), (float)(img.Height - (img.Height * _probabilities[intRange, i] / maxProb)));
                // Console.Out.WriteLine("point: " + points[i].X + "," + points[i].Y);
            }
            g.DrawLines(Pens.Black, points);

            g.Save();
            g.Dispose();
            return img;
        }

        /// <summary>
        /// Creates a test data set to test the learning function
        /// </summary>
        public void TestLearning()
        {
            int numReadings = 100;
            double[] hitDistances = new double[numReadings];
            double[] distances = new double[numReadings];
            int distanceNoise = 50;
            Random r = new Random();
            for (int i = 0; i < numReadings; i++)
            {
                hitDistances[i] = Math.Max(1, (double) r.Next((int)Math.Round(_maxRange)));
                distances[i] = Math.Min(_maxRange, Math.Max(1, hitDistances[i] + r.NextDouble() * distanceNoise - distanceNoise));
                
                // some max range noise
                if (r.Next(20) == 1)
                {
                    distances[i] = _maxRange;
                }
            }

            LearnParameters(hitDistances, distances);
        }

        /// <summary>
        /// This function learns the beam error parameters from a set of true
        /// hit distances and a set of noisy ditances.
        /// </summary>
        /// <param name="hitDistances">The true distances</param>
        /// <param name="distances">The noisy distances</param>
        public void LearnParameters(double[] hitDistances, double[] distances )
        {
            double[] mixHits = new double[distances.Length];
            double[] mixShorts = new double[distances.Length];
            double[] mixRands = new double[distances.Length];
            double[] mixFails = new double[distances.Length];
            bool finished = false;

            for (int l = 0; l < distances.Length; l++)
            {   
                double mixHit;
                double mixShort;
                double mixRand;
                double mixFail;
                double hitVariance;
                double shortLambda;

                double totHit=0;
                double totShort=0;
                double totRand=0;
                double totFail=0;

                double rangeDifWeightSum = 0;
                double shortRangeWeightSum = 0;

                double hitVarianceChange;
                double shortLambdaChange;

                for (int i = 0; i < distances.Length; i++)
                {
                    int hitDistance = (int) Math.Round(hitDistances[i]);
                    int testDistance = (int) Math.Round(distances[i]);
                    double normalizer = 1.0 / (_hitProbabilities[hitDistance, testDistance] +
                       _shortProbabilities[hitDistance, testDistance] +
                        _randomProbability +
                         (testDistance == _maxRange ? 1.0 : 0));

                    mixHits[i] = normalizer * _hitProbabilities[hitDistance, testDistance];
                    mixShorts[i] = normalizer * _shortProbabilities[hitDistance, testDistance];
                    mixRands[i] = normalizer * _randomProbability;
                    mixFails[i] = normalizer * (testDistance == _maxRange ? 1.0 : 0);

                    //if (mixHits[i] > 1)
                    //    Console.Out.WriteLine("hit exceeded max" + hitDistance + "," + testDistance + "," + mixHits[i] + "," + mixShorts[i] + "," + mixRands[i] + "," + mixFails[i]);
                    totHit += mixHits[i];
                    totShort += mixShorts[i];
                    totRand += mixRands[i];
                    totFail += mixFails[i];

                    rangeDifWeightSum += mixHits[i]*Math.Pow(hitDistance - testDistance, 2);
                    shortRangeWeightSum += mixShorts[i]*testDistance;
                }

                mixHit = totHit/(1.0*distances.Length);
                mixShort = totShort/(1.0*distances.Length);
                mixRand = totRand/(1.0*distances.Length);
                mixFail = totFail/(1.0*distances.Length);

               // if (mixFail + mixRand + mixShort + mixHit > 1)
              //  {
              //      Console.Out.WriteLine("Mix greater than 1: " + mixFail + "," + mixRand + "," + mixShort + "," + mixHit);
             //  }

                hitVariance = (1/totHit)*rangeDifWeightSum;
                shortLambda = totShort / shortRangeWeightSum;

                // check for convergance
                hitVarianceChange = Math.Abs((_beamHitVariance - hitVariance)/_beamHitVariance);
                shortLambdaChange = Math.Abs((_beamShortDecay - shortLambda) / _beamShortDecay);
                
                if (hitVarianceChange + shortLambdaChange < 0.01)
                {
                    Console.Out.WriteLine("Less than 1% change");
                    finished = true;
                }

                // set intrinsics
                _beamHitVariance = hitVariance;
                _beamShortDecay = shortLambda;
                _mixHit = mixHit;
                _mixShort = mixShort;
                _mixRandom = mixRand;
                _mixFailure = mixFail;
                 Prepare();
                if(finished)
                {
                    Console.Out.WriteLine("Final Mixes " + _mixHit + "," + _mixShort + "," + _mixRandom + "," + _mixFailure + " var:" + _beamHitVariance + " lambda:" + _beamShortDecay);
                    break;
                } 
                else
                {
                    Console.Out.WriteLine("Mixes " + _mixHit + "," + _mixShort + "," + _mixRandom + "," + _mixFailure + " var:" + _beamHitVariance + " lambda:" + _beamShortDecay);
                }
            }
        }
    }


}
